#include "motorControl.h"

MotorControl::MotorControl(int motorCount, int stepPins[], int dirPins[])
{
  this->motorCount = motorCount;
  this->stepPins = stepPins;
  this->dirPins = dirPins;

  this->motorStates = new State[motorCount];
  this->motorSteps = new int[motorCount];
  this->motorSpeeds = new int[motorCount];

  for (int i = 0; i < motorCount; i++)
  {
    this->motorStates[i] = STOP;
    this->motorSteps[i] = 0;
    this->motorSpeeds[i] = 0;
  }

  // initialize the pins
  for (int i = 0; i < motorCount; i++)
  {
    pinMode(stepPins[i], OUTPUT);
    pinMode(dirPins[i], OUTPUT);
  }
}

MotorControl::~MotorControl()
{
  delete[] this->motorStates;
  delete[] this->motorSteps;
  delete[] this->motorSpeeds;
}

bool MotorControl::moveMotor(int targetMotor, int targetSteps, int speed)
{
  // check if the target motor is valid
  if (targetMotor < 0 || targetMotor >= this->motorCount)
    return false;
  if (targetSteps < 0)
    return false;

  // calculate the direction
  if (speed > 0)
  {
    digitalWrite(this->dirPins[targetMotor], HIGH); // clockwise
  }
  else
  {
    digitalWrite(this->dirPins[targetMotor], LOW); // counterclockwise
    speed = -speed;                                // make the steps positive
  }

  // send the pulse signal

  for (int i = 0; i < targetSteps; i++)
  {
    digitalWrite(this->stepPins[targetMotor], HIGH);
    delayMicroseconds(1000000 / speed / 2); // high level duration
    digitalWrite(this->stepPins[targetMotor], LOW);
    delayMicroseconds(1000000 / speed / 2); // low level duration
  }

  return true;
}

/**
 * Process incoming serial commands to control motors.
 *
 * This function reads commands from the serial buffer, parses them, and controls the specified motor accordingly.
 * The supported command formats are:
 * - M<motor_id>:<steps>          // Move the motor by a specified number of steps at the default speed (1000us/step).
 * - M<motor_id>:<steps>:<speed>  // Move the motor by a specified number of steps at a specified speed (in us/step).
 *
 * If the command format is invalid, an error message is sent back via the serial port.
 */
void MotorControl::processSerialCommand(String command)
{
  // M<motor_id>:<steps>:<speed>
  if (command.startsWith("M") && command.indexOf(":") != -1)
  {
    int motorId = command.substring(1, command.indexOf(":")).toInt();
    int steps = 0;
    // default speed set to 1000us/step
    int speed = 1000;

    // get steps and speed
    int secondColonIndex = command.indexOf(":", command.indexOf(":") + 1);
    if (secondColonIndex != -1)
    {
      steps = command.substring(command.indexOf(":") + 1, secondColonIndex).toInt();
      speed = command.substring(secondColonIndex + 1).toInt();
    }
    else
      steps = command.substring(command.indexOf(":") + 1).toInt();

    // set State
    if (steps == -1)
    {
      this->motorStates[motorId] = CONTIUOUS;
      this->motorSteps[motorId] = 0;
      this->motorSpeeds[motorId] = speed;
      // this->log();
    }
    else
    {
      this->motorStates[motorId] = STEP;
      this->motorSteps[motorId] = steps;
      this->motorSpeeds[motorId] = speed;
      // this->log();
    }
  }
  // stop:<id> or stop
  else if (command.startsWith("stop"))
  {
    int stopId = -1; // default value -1 indicates stopping all motors
    int colonIndex = command.indexOf(":");
    if (colonIndex != -1)
    {
      // If a motor ID is specified, parse the ID
      stopId = command.substring(colonIndex + 1).toInt();
    }

    // Stop all motors
    if (stopId == -1)
    {
      for (int i = 0; i < this->motorCount; i++) // Assuming motorCount is the total number of motors
      {
        this->motorStates[i] = STOP; // Set motor state to stopped
        this->motorSteps[i] = 0;
        this->motorSpeeds[i] = 0;
      }
    }
    // Stop the specified motor
    else
    {
      if (stopId >= 0 && stopId < this->motorCount) // Ensure the ID is within the valid range
      {
        this->motorStates[stopId] = STOP;
        this->motorSteps[stopId] = 0;
        this->motorSpeeds[stopId] = 0;
      }
    }
  }
  else
    Serial.println("Invalid command format. Use 'M<motor_id>:<steps>' or 'M<motor_id>:<steps>:<speed>'");
  this->log();
}

void MotorControl::loop()
{
  if (Serial.available() > 0)
  {
    String command = Serial.readStringUntil('\n');
    command.trim();
    this->processSerialCommand(command);
  }
  // Finite-state machine
  for (int i = 0; i < this->motorCount; i++)
  {
    switch (this->motorStates[i])
    {
    case STOP:
      break;
    case STEP:
      this->moveMotor(i, this->motorSteps[i], this->motorSpeeds[i]);
      this->motorStates[i] = STOP;
      this->motorSteps[i] = 0;
      this->motorSpeeds[i] = 0;
      this->log();
      break;
    case CONTIUOUS:
      this->moveMotor(i, 1, this->motorSpeeds[i]);
      break;
    }
  }
}

void MotorControl::log()
{
  for (int i = 0; i < this->motorCount; i++)
  {
    Serial.print("Motor ");
    Serial.print(i);
    Serial.print(": ");
    Serial.print("Steps: ");
    Serial.print(this->motorSteps[i]);
    Serial.print(", Speed: ");
    Serial.print(this->motorSpeeds[i]);
    Serial.print(", State: ");
    Serial.println(this->motorStates[i]);
  }
}